import sys
import time
sys.path.append("../")
from drivers.driver_serial import *

def parse_magnetic_data(frame):
    """解析带帧头、帧尾和校验和的16字节数据帧，支持有符号数据"""
    if len(frame) != 28:
        return None, "Invalid frame length"
    
    # 1. 校验帧头
    if frame[0] != 0xAA or frame[1] != 0x55:
        return None, "Header mismatch"
    
    # 2. 校验帧尾
    if frame[27] != 0x0D:
        return None, "Tail mismatch"
    
    # 3. 计算校验和（异或校验）
    checksum = 0
    for i in range(2, 26):  # 数据部分为2-13字节（共12字节数据）
        checksum ^= frame[i]
    
    if checksum != frame[26]:
        return None, f"Checksum error (calculated: {hex(checksum)}, received: {hex(frame[14])})"
    
    # 4. 解析数据并进行符号扩展
    def signed16(value):
        """将16位无符号值转换为有符号int16_t"""
        return value if value < 0x8000 else value - 0x10000
    
    # 解析S1传感器数据
    s1x_raw = (frame[2] << 8) | frame[3]
    s1y_raw = (frame[4] << 8) | frame[5]
    s1z_raw = (frame[6] << 8) | frame[7]
    
    # 解析S2传感器数据
    s2x_raw = (frame[8] << 8) | frame[9]
    s2y_raw = (frame[10] << 8) | frame[11]
    s2z_raw = (frame[12] << 8) | frame[13]
    
    # 解析S1传感器数据
    s3x_raw = (frame[14] << 8) | frame[15]
    s3y_raw = (frame[16] << 8) | frame[17]
    s3z_raw = (frame[18] << 8) | frame[19]
    
    # 解析S2传感器数据
    s4x_raw = (frame[20] << 8) | frame[21]
    s4y_raw = (frame[22] << 8) | frame[23]
    s4z_raw = (frame[24] << 8) | frame[25]    
    
    
    # 应用符号扩展
    s1xMag = signed16(s1x_raw)
    s1yMag = signed16(s1y_raw)
    s1zMag = signed16(s1z_raw)
    s2xMag = signed16(s2x_raw)
    s2yMag = signed16(s2y_raw)
    s2zMag = signed16(s2z_raw)
    s3xMag = signed16(s3x_raw)
    s3yMag = signed16(s3y_raw)
    s3zMag = signed16(s3z_raw)
    s4xMag = signed16(s4x_raw)
    s4yMag = signed16(s4y_raw)
    s4zMag = signed16(s4z_raw)
    
    return {
        "s1x": s1xMag, "s1y": s1yMag, "s1z": s1zMag,
        "s2x": s2xMag, "s2y": s2yMag, "s2z": s2zMag,
        "s3x": s3xMag, "s3y": s3yMag, "s3z": s3zMag,
        "s4x": s4xMag, "s4y": s4yMag, "s4z": s4zMag,        
    }, "OK"

def receive_continuous_data(serial_device, timeout=0.2):
    """连续接收数据，处理分包和帧同步（支持16字节帧）"""
    buffer = b""  # 接收缓冲区
    expected_frame_len = 28  # 期望的帧长度（含帧头、数据、校验和、帧尾）
    last_frame_time = time.time()  # 记录最后接收帧的时间
    
    while True:
        try:
            # 非阻塞读取所有可用字节
            data = serial_device.read_bytes()
            if not data:
                # 无数据时根据超时时间休眠，避免CPU占用过高
                if time.time() - last_frame_time > 1.0:
                    print("Waiting for data...")
                time.sleep(timeout)
                continue
            
            buffer += data
            print(f"Received raw: {[hex(b) for b in buffer]}")
            
            # 5. 帧同步逻辑：寻找帧头0xAA 0x55
            while True:
                # 查找帧头位置
                header_pos = buffer.find(b'\xAA\x55')
                if header_pos == -1:
                    break  # 未找到帧头，等待后续数据
                
                # 确保缓冲区有足够字节（16字节）
                if len(buffer) >= header_pos + expected_frame_len:
                    frame = buffer[header_pos:header_pos+expected_frame_len]
                    buffer = buffer[header_pos+expected_frame_len:]
                    last_frame_time = time.time()  # 更新最后接收时间
                    
                    # 解析数据
                    result, status = parse_magnetic_data(frame)
                    if result:
                        print(f"[VALID] s1: X={result['s1x']}, Y={result['s1y']}, Z={result['s1z']}")
                        print(f"[VALID] s2: X={result['s2x']}, Y={result['s2y']}, Z={result['s2z']}")
                        print(f"[VALID] s3: X={result['s3x']}, Y={result['s3y']}, Z={result['s3z']}")
                        print(f"[VALID] s4: X={result['s4x']}, Y={result['s4y']}, Z={result['s4z']}")
                    else:
                        print(f"[ERROR] {status}")
                else:
                    break  # 数据不足，等待后续字节
        
        except KeyboardInterrupt:
            print("User interrupted, exiting...")
            break
        except Exception as e:
            print(f"Error: {str(e)}")
            time.sleep(1)  # 出错后暂停1秒再重试

if __name__ == "__main__":
    # 1. 扫描可用串口
    serial_ports = scan_serial_ports()
    if not serial_ports:
        print("No serial ports found.")
        sys.exit(1)
    
    # 2. 选择串口（可修改为指定端口）
    port = serial_ports[0][0]
    print(f"Connecting to {port}...")
    
    # 3. 初始化串口设备（波特率需与单片机一致）
    sp = SerialDevice(port, baud_rate=1000000, timeout=0.5)  # 波特率与单片机匹配
    success, msg = sp.open()
    if not success:
        print(f"Failed to open port: {msg}")
        sys.exit(1)
    
    print(f"Serial port {port} opened, waiting for data (16-byte frames)...")
    
    # 4. 启动连续接收
    receive_continuous_data(sp)
    
    # 5. 程序结束时关闭串口
    sp.close()